#include "ioregister.hpp"

IORegister::IORegister(int address, int bank, int standard)
    : Register(address, bank, standard)
{
    QMutexLocker(this->mutex);
    this->value = -1;
    this->tris = new Register(-2, -2, 255);
    this->tris->reset();
    this->latch = new Register(-2, -2, 0);
    this->latch->reset();

    connect(this->tris, SIGNAL(contentChanged(int)), this, SLOT(updateOutput(int)));
}


IORegister::~IORegister()
{
    delete latch;
}


void IORegister::setTrisValue(int value)
{
    QMutexLocker(this->mutex);
    int old = this->tris->getValue();

    int outputmask = old ^ 255;
    this->latch->setValue(this->value & outputmask);

    this->tris->setValue(value);
    this->updateOutput(old);
}


int IORegister::getTrisValue() const
{
    QMutexLocker(this->mutex);
    return this->tris->getValue();
}


void IORegister::setTrisBit(int bit)
{
    QMutexLocker(this->mutex);
    if (!this->tris->isSet(bit))
    {
	if (this->isSet(bit))
	    this->latch->setBit(bit);
	this->clearBit(bit);

	this->tris->setBit(bit);
    }
}


void IORegister::clearTrisBit(int bit)
{
    QMutexLocker(this->mutex);
    if (this->tris->isSet(bit))
    {
	this->clearBit(bit);

	this->tris->clearBit(bit);
    }
}


bool IORegister::trisIsSet(int bit) const
{
    QMutexLocker(this->mutex);
    return this->tris->isSet(bit);
}


void IORegister::clearTris()
{
    QMutexLocker(this->mutex);
    int old = this->tris->getValue();

    this->tris->clear();
    this->updateOutput(old);
}


void IORegister::setTris(Register* trisRegister)
{
    QMutexLocker(this->mutex);
    disconnect(this->tris, SIGNAL(contentChanged(int)), this, SLOT(updateOutput(int)));
    delete this->tris;

    this->tris = trisRegister;
    connect(this->tris, SIGNAL(contentChanged(int)), this, SLOT(updateOutput(int)));
}


bool IORegister::isInput(int bit) const
{
    QMutexLocker(this->mutex);
    return this->tris->isSet(bit);
}


bool IORegister::latchIsSet(int bit) const
{
    QMutexLocker(this->mutex);
    return this->latch->isSet(bit);
}


void IORegister::setLatchValue(int value)
{
    QMutexLocker(this->mutex);
    this->latch->setValue(value);
}

int IORegister::getLatchValue() const
{
    QMutexLocker(this->mutex);
    return this->latch->getValue();
}


void IORegister::setLatchBit(int bit)
{
    QMutexLocker(this->mutex);
    this->latch->setBit(bit);
}


bool IORegister::getLatchBit(int bit) const
{
    QMutexLocker(this->mutex);
    return this->latch->isSet(bit);
}


void IORegister::clearLatchBit(int bit)
{
    QMutexLocker(this->mutex);
    this->latch->clearBit(bit);
}


void IORegister::clearLatch()
{
    QMutexLocker(this->mutex);
    this->latch->clear();
}


void IORegister::setValue(int value, bool force)
{
    QMutexLocker(this->mutex);
    int newVal;

    if (force)
    {
	newVal = value;

    } else {
	int changed = value ^ this->getValue(); //Get bits which have changed
	int toLatch = changed & this->tris->getValue(); //Save bits to Latch if written to input pin
	int newLatch = toLatch | this->latch->getValue();
	int toOut = changed & (toLatch ^ 255); //Get the new values for changed pins
	newVal = this->getValue() & (changed ^ 255); //Get unchanged pins
	newVal |= toOut;

	this->latch->setValue(newLatch);
    }

    if (this->value != newVal)
    {
	this->value = newVal;
	emit changed(this->address, this->bank);
    }
}


void IORegister::setBit(int bit)
{
    QMutexLocker(this->mutex);
    if (this->tris->isSet(bit))
	this->latch->setBit(bit);
    else
	Register::setBit(bit);
}


void IORegister::clearBit(int bit)
{
    QMutexLocker(this->mutex);
    if (this->tris->isSet(bit))
	this->latch->clearBit(bit);
    else
	Register::clearBit(bit);
}


void IORegister::clear()
{
    QMutexLocker(this->mutex);
    if (this->latch->getValue() != 0)
    {
	this->value = this->value & this->tris->getValue();
	this->latch->clear();
	emit changed(this->address, this->bank);
    }
}


QString IORegister::trisToString(int bit) const
{
    return this->tris->toString(bit);
}


QString IORegister::latchToString(int bit) const
{
    return this->latch->toString(bit);
}


void IORegister::setInput(int bit)
{
    QMutexLocker(this->mutex);
    if (bit < 0 || bit > 7)
	return;

    /* TODO: Nachfrage, ob speichern im Latch beim Umschalten */
    if (this->isSet(bit))
	this->latch->setBit(bit);

    this->tris->setBit(bit);
    this->clearBit(bit);
}


void IORegister::setOutput(int bit)
{
    QMutexLocker(this->mutex);
    if (bit < 0 || bit > 7)
	return;

    this->tris->clearBit(bit);

    if (this->latch->isSet(bit))
    {
	this->setBit(bit);
	this->latch->clearBit(bit);
    } else
	this->clearBit(bit);
}


void IORegister::updateOutput(int oldValue)
{
    if (oldValue < 0)
	return;

    QMutexLocker(this->mutex);
    int changemask = oldValue ^ this->tris->getValue(); //Get changed bits
    int outToIn = changemask & this->tris->getValue(); //Get bits which where output and are now input
    int inToOut = changemask & (this->tris->getValue() ^ 255); //Get bits which switched from in to out
    int fromLatch = this->latch->getValue() & inToOut; //Get latch values for new output ports
    int toLatch = this->value & outToIn; //Get current values of old input ports
    int newValue = this->value & (changemask ^ 255); //Set bits which changed to zero
    newValue |= fromLatch;
    int newLatch = this->latch->getValue() & (changemask ^ 255);
    newLatch |= toLatch;
    this->latch->setValue(newLatch);
    this->setValue(newValue, true);
}
